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Abstract — We investigate several coordinate systems and dy- 
namical vector fields for target tracking to be used in driver 
assistance systems. We show how to express the discrete dynamics 
of maneuvering target vehicles in arbitrary coordinates starting 
from the target's and the own (ego) vehicle's assumed dynamical 
model in global coordinates. We clarify the notion of "ego 
compensation" and show how non-inertial effects are to be 
included when using a body-fixed coordinate system for target 
tracking. We finally compare the tracking error of different 
combinations of target tracking coordinates and dynamical vector 
fields for simulated data. 

I. INTRODUCTION 

Driver assistance systems (DAS) such as adaptive cruise 
control (ACC) or lane departure warning (LDW) need to 
perceive the environment using exteroceptive sensors (e. g. 
radar or laser for ACC and camera for LDW). As DAS be- 
come more sophisticated and move from comfort applications 
to safety -critical applications such as automatic emergency 
braking, the requirements regarding perception are becoming 
more stringent. An essential part of exteroception is the target 
dynamics used in the temporal prediction of observers with 
a predictor-corrector structure. In this paper different target 
dynamics in different coordinate systems will be derived 
and assessed with respect to satisfactory modeling of target 
dynamics and observability. 

The modeling of target dynamics without an explicit es- 
timation of the target driver's intent for DAS is generally 
approximated by assuming non-maneuver or maneuver models 
JD> El of varying degrees of complexity where the target 
driver's actions like accelerating or steering are subsumed into 
(Gaussian) noise processes: 

with £* the target state vector in global coordinates, u l the 
input (control) vector, and v* a multi-dimensional stochastic 
process. Those models are usually formulated with respect to 
a global, inertial reference framaM tangential to the earth's 
surface. This also applies to the modeling of the ownship 
("ego") dynamics, £|S° = f e9 °{Q 9 °, u 6 g 9 °, v e g 9°) however 
the corresponding observer can be fed with proprioceptive 
measurements such as yaw rate, lateral acceleration, or longi- 
tudinal velocity coming from vehicle stability control (VSC) 

'The effect of the earth's rotation around its polar axis as well as its rotation 
around the sun, etc on the vehicle motion can safely be neglected, hence an 
earth fixed reference frame will be called "inertial" in this paper. 



systems. Since in both target and ego modeling the input is 
usually zero, we will suppress the input vectors u in these 
vector fields from now on. 

Estimation of the target dynamics is based on exteroception 
by radar, laser, or video sensors that provide measurements ( 
relative to the ego vehicle. Hence somewhere in the estimation 
process a transformation from relative to global coordinates 
must be performed^] A dynamical system for the combined 
target and ego observer would then have the following form 
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where the w's are stochastic measurement processes. Since the 
measurements £* are relative to the ego vehicle, the output 
function h* must contain a control vector w* = ^ 9 ° in order 
to map the relative measurements onto the global target state 
£'. As all relative measurements contain some sort of position 
information, the ego state ^ go must hence also estimate the 
position of the ego vehicle. 

This would be an appropriate approach if the ego state 
£? 9 ° were fully observable which implies absolute position 
measurements by e. g. GPS receivers. If £| 9 ° were not fully 
observable its covariance would grow without bounds which 
would cause the covariance matrix of £* to grow infinitely 
by propagation through the output function /i*. This means 
that the dynamical system not stochastically observable - 
a necessary condition for the convergence of an extended 
Kalman filter (EKF), see e. g. @). Since most vehicles are 
not equipped with a GPS receiver, their position can only be 
estimated by dead-reckoning and is therefore unobservable. 
Even if vehicles are equipped with a GPS receiver e. g. from 
their navigation system, the GPS position (without differential 
corrections) is only accurate to about ±5 . . . 15m Q. This 
error would then be propagated to unacceptably large position 
covariances in £' While the covariance of the relative position 
in this case might remain bounded as suggested in [6| it 

2 Modeling the target dynamics a priori in coordinates relative to the ego 
vehicle £' , = f(P ,,v t ,) is rather unattractive and not considered here 
since e. g. a constant acceleration model for / would imply that the target 
permanently moves with a constant acceleration plus noise relative to the ego 
vehicle irrespective of the actual state and motion of the ego vehicle. 
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Fig. 1. Inertial and ego-fixed coordinate frames. The origin of the ego-fixed 
frame and the reference point of the target vehicle are common choices but 
are by no means unique. 



is not advisable to work with unobservable, non-convergent 
systems whose ever-growing covariances will also invalidate 
the propagation of covariance by linearization as in an EKF. 
Hence we propose formulating the target dynamics in rel- 
ative, i. e. in ego vehicle fixed coordinates. The goal is to 
replace the combined dynamical system (fill with a system that 
contains £} rel and £? 9 ° where the unobservable states of £| 9 ° 
are not used for the estimation of £* e ;. In the next section it will 
be shown how to derive the relative target dynamics starting 
from the global target and ego dynamics in eq. [T] While the 
use of relative target dynamics for automotive target tracking is 
not new (see e. g. 0, 0, (9)), in this paper a general, system- 
theoretic framework for the rigorous derivation of relative 
target dynamics starting from arbitrary dynamical vector fields 
for global target and ego dynamics is presented. This includes 
the derivation of the discrete dynamics with process and 
input noise covariance matrices as needed for an EKF as 
observer. The derivation is illustrated by three different choices 
of coordinate systems and vector fields and their accuracy 
in tracking targets is assessed by numerical simulation. The 
observability of the combined target and ego dynamics is also 
discussed. 

II. RELATIVE TARGET VEHICLE DYNAMICS 

A. Derivation 

In order to derive the discrete target dynamics in relative 
coordinates we first need to obtain its continuous vector field. 
This requires a definition of the state vectors for the global 
target and ego dynamics and their continuous vector fields 
as well as a definition of the relative coordinates. The vector 
fields are given by 



n 9 ° = r s °(C">r) 



(2) 



e 9 = n>i) 

where we assume that no control inputs are necessary for f ego 
and /*. We now define a new state by 



&a = m(£,Z go ) 



(3) 



where m is in general a nonlinear function that depends upon 
the choice of the coordinates of £* £^ 9 ° (Cartesian, polar, etc). 



If both £* and £^ 9 ° are in Cartesian coordinates and the new 
coordinates are also Cartesian coordinates for a ego body fixed 
system we get the more intuitive expression 



m(C,C 9 °) 



M(^9° 
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where M(£g 3 °) contains a rotation to the ego-fixed coordinate 
system as well as corrections due to the fact that the ego-fixed 
system is a non -inert ial system and thus experiences pseudo- 
forces, see app. A 3 By taking the time derivative of eq. (Wj) 



we get a vector field for £* 



However, the goal of this computation is to replace £' 
with £* ei ; therefore we need to use eq. (Wb again in order 
to eliminate £*: 
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The combined system of differential equations for £* ei 
P e J>° reads 
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This system of differential equations governs the dynamics of 
the target vehicle relative to the ego vehicle and are based 
upon: the dynamics of the target vehicle with respect to the 
ground /*, the dynamics of the ego vehicle with respect to 
the ground f ego , and the definition of the relative coordinates 
m. This procedure separates dynamical models for individual 
vehicle dynamics (ego or target) from the relative dynamics 
used for tracking in an arbitrary coordinate system. The "ego 
compensation" at the level of continuous dynamics is not a 
separate step but is intertwined with the relative dynamics. 

B. Solution 

In this paper, all stochastic differential equations are chosen 
to be solved by the discrete-time counterpart method 
where the continuous stochastic process v g (t) is replaced by 
a discrete stochastic process v k which is constant from one 
time step to the nextrl By abuse of notation, here v also 
denotes this constant value. 

Since the ego dynamics is decoupled from the relative target 
dynamics, it can be solved first, see app. [B] and its solution 

t%>°{t) = F*9°(^o(t ),t - to,** 30 

the differential equation for £* 
differential equation: 



can be inserted into 
j to arrive at a time-dependent 
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3 From now on, we suppress the dependence of M on £g 9 ° in our notation. 
Alternatively, the discrete-time equivalent method (T) can be employed 
using the power spectral density of the continuous stochastic process. 



The solution of this differential equation - if it exists - can be 
cast into the notation of discrete time systems to be used later 
for application of the EKF: 
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where At k = t k +i — t k is the time difference from one 
iteration to the next and u* el k — (y 1 fc , vt 9 k ) T is the effective 
discrete stochastic process for the relative target dynamics. 
Note that Cfk+i does not nave inputs but £* ei k+1 has the 
ego state ^ e9 k as its input or control vector. 

In the following we will assume that the ego estimation 
using proprioceptive measurements from VSC sensors is sep- 
arate from the exteroception and only outputs the ego state 
£ eg ? and its covariance matrix P &9 Z . 

^g k g k 

For the use of eq. ([8]l in an EKF, we define the matrices 



A k 
B k 
G k 



d e r ^ F rel(Cel feJ^fc) At k , V rel k ) 
d e g S ° F rel(£.rel fci C g 9 °k, At kl V rel k ) 
Vv 1 FrelVirel fc J C k'^k,V rel k ) 



which are used for the propagation of the state (A k ), input 
{Bk), and process noise (G k ) covariances by linearization. The 
input noise and process noise covariance matrices are then 



ynput = Bk pe9° B 



g k - 



^process s~< rrt 
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where V* el k 



cov(^' e ; k ). Since nonlinear effects in esti- 
mation are not the focus of this paper we content ourselves 
in the numerical analysis with the standard EKTH unlike 
in J3) where the unscented Kalman filter was used. Note 
however, that even the unscented transformation does not 
fully propagate the second moments of a multi-dimensional 
probability distribution [10]. 

C. Vehicle dynamics 

In eq. (roj arbitrary vector fields /' and f ego can be used, 
in this paper, however, we focus on two common object 
dynamics, namely the white noise jerk model (see e. g. |Q~|) 
and the constant turn rate and acceleration model (CTRA, 
see app. IB). While the former might be advantageous in the 
object initialization stage if velocity and/or acceleration are 
not directly measured and are initialized by default values or 
if not just vehicles but also other objects such as pedestrians 
are perceived, the latter better represents the non-holonomic 
behavior of wheeled vehicles and also performed best in a 
comparative study of vehicle motion models J3). For the ego 
dynamics only the CTRA model is considered in this paper. 

5 The local convergence of the EKF for nonlinear discrete-time systems has 
been proven in |4| under certain conditions. 



D. Relative coordinate systems 

After choosing the target and ego dynamics the relative 
coordinate system for target tracking m must be specified. 
We will investigate two different coordinate systems, one in 
which the entire target motion is relative to the ego vehicle 
(pure relative coordinates) and one in which the target position 
is estimated relative to the ego vehicle whereas the velocity 
and acceleration are estimated over ground, but rotated into 
the ego coordinate system ("mixed" coordinates). We will not 
dwell on other aspects of coordinate system choices such as the 
performance of polar versus Cartesian coordinates for target 
tracking as in ifTTl . 

III. Examples 

A. Tracking in relative coordinates, white noise jerk model 

For this example the global target dynamics is the non- 
maneuver white noise jerk model. It is naturally expressed in 
Cartesian coordinates, see e. g. Q). The ego dynamics is given 
by the CTRA model. The relative dynamics is expressed in 
Cartesian coordinates. In order to apply eq. d3), the ego state 
vector must be transformed to Cartesian coordinates as well. 
With this choice the function m reads 

e e i=rn(e g ,e g 9o )=M(e g n(e g -e g 9 °) 

as in Q and we obtain as in §5$ 

H el = MM- i e rel 

+M (f(M-^rel + C g 9 °, y g ) - f 90 (e g 90 , < 9 °)) 



Using the ego trajectory eq. (16i and defining £* 
(xyxyxy) we obtain 
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Note that the non-trivial components of this differential equa- 
tion are - as expected - the highest derivatives; everything else 
is purely kinematic. This would not have been the case had 
we omitted the non-inertial contributions in M, 

In eq. d7j) the target state vector £* e/ is rotated into the ego 
coordinate system. This rotation, however, is not applied to the 



target process noise v l . On the other hand, the relative target 
dynamics should not depend on the orientation of an arbitrary 
global coordinate system. In (Nb the terms proportional to i/£> 
and i>t-- „ depend on the glooal orientation ifj^ 90 of the ego 
vehicle. It can be checked, however, that by choosing the 
process noise to be isotropic, i. e. with identical covariance 
values for v$% and v$r , the process covariance matrix 
Qprocess b ecomes independent of ipQ 9 °. 

This differential equation is of the form £ = A£ + B(t) and 
can therefore be solved by the standard formula for linear time- 
invariant systems which is also valid for time -variant B(t), see 
e. g. [12]. Since the solution £' e; fe+1 and the corresponding 
Jacobians A/., Bt, and Gk are rather unwieldy expressions 
which can easily be computed by standard symbolic compu- 
tation engines such as Matlab's symbolic toolbox, we will not 
provide them here. 

B. Tracking in mixed coordinates, white noise jerk model 

For this example the global target dynamics is again the 
non-maneuver white noise jerk model. However, in "mixed" 
coordinates, velocities and accelerations are the inertial quan- 
tities measured over ground, rotated into the ego coordinate 
system. This has the advantage that the dynamics of velocity 
and acceleration is reduced in the sense that the range of 
values of the velocity and acceleration is cut in half. This 
is particularly important for the object initialization of not 
measured states like acceleration. 

Again we transform the ego state vector to Cartesian co- 
ordinates: £|s° = (x e9 ° y ego x e9 ° y ego x ego y ego ) T . 
Then we use matrix M without non-inertial terms since the 
velocities and accelerations are now inertial quantities and 
introduce a projector II to project out the ego velocities and 
accelerations 
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Using the ego trajectory eq. ( 16 1 and redefining £. 
(x y x y x y) we obtain 
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As in d9|i, the terms proportional to !/&• and iA-- n depend on 
the glooal orientation i/jq 9 ° of the ego vehicle. By choosing 
the process noise to be isotropic the process covariance matrix 
again becomes independent of ipQ 9 °- 

This differential equation is also of the form £ = A£ + B(i) 
and can be solved by linear system techniques. Again we will 
not provide the unwieldy but easily computable solution. 

C. Tracking in mixed coordinates, CTRA model 

Using the CTRA model for the representation of the target 
dynamics, it is more convenient not to transform to Carte- 
sian coordinates. Hence both ^t 9 ° and £* are interpreted as 

(x re i v re i tbr-fi ii>,r v„ cl„) . The relative coordinates 



yrel Iprel ~>Pg V g 

are the relative x re i and y re i position with respect to the 
ego vehicle coordinate system, the angle between the ego 
and target velocities over ground ip re i which coincides for 
the CTRA models with the relative angle of the vehicle 
orientations, the target yaw rate over ground ip g and the target 
speed v g and longitudinal acceleration a g over ground. 
The coordinate transformation for this setting is 

e mix = m(e g ,e g 9 °) = m g n (c - n^°) 
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The resulting differential equation reads 
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Using the ego trajectory eq. (16i and defining £ 
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This non-linear differential equation can be solved as follows: 



the lower four entries of (12i can easily be solved by direct 



integration. Those expressions can now be inserted into the 
first two components. They are then of the form £ = At; + 
B(t) and can be solved by linear system techniques. Since 
terms proportional to t 2 appear inside the sine and cosine, 
integration results in Fresnel integrals. We therefore expand 
the integrand to first order in iA- for analytically tractable 
expressions. Again, the explicit solutions are not provided for 
lack of space, however by setting the accelerations a Si o and 



<i 



ego 



as well as the process noises v 



I>; 



eqo 



to zero, the state update expressions as in |9) are recovered. 
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Fig. 2. Block diagram of the simulation setup. 



IV. NUMERICAL RESULTS 

A. Simulation setup 

In order to assess the estimation accuracy of the above 
dynamical models in different coordinate systems, a nu- 
merical study was performed. First, 50 trajectories with a 
duration of 20s for the ego as well as the target vehicle 
were generated using the CTRA model described in app. [B] 
The process noise for both CTRA models was chosen to 
be cov(^ k ,^,k) = diag(l(^) 2 , 25(f) 2 ). In order to 
perturb the reference trajectories away from the CTRA model, 
noise was added to the tp component. From these reference 
trajectories, proprioceptive and exteroceptive measurements 
corrupted with additive white noise are extracted every Atk = 
40ms - a typical value for radar or laser measurements. Then 
the exteroceptive measurements are fed into three extended 
Kalman filters using the three discrete dynamics introduced 
see fig. 13] The contribution of the process 



III 



in section 

noise from the ego dynamics co!)(v-- J ° 1/^°) as well as of 
the relative target dynamics using the CTRA in model C 
cov(v% , , v\ t ) were set to the above values for the reference 
trajectory generation. The contribution of the relative target 
dynamics using the white noise jerk dynamics in models A 
and B were determined by numerically computing the values 
of awH.fc.^J « diag (325(f) 2 , 325(f) 2 ) over all 
50 reference trajectories. 

For track initialization, the not-measured entries of the state 
vector were set to zero. The proprioceptive measurements are 
fed into an EKF using the CTRA model for the estimation 
of the ego trajectory. The output of the proprioception serves 
as an input or control vector for the exteroceptive observers. 
Finally the Euclidean error of the position estimation with 
respect to the reference trajectories is determined. 

B. Comparison of estimation errors 

At every time step k of every trajectory j, the Euclidean 
error of the estimated relative position 
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Model 


av J (max fe (A ; , fc )) 


av,-(avfc(Aj*.)) 


101 


A 
B 
C 


7.60 
4.35 
4.17 


1.52 
1.02 
0.95 


1.6 • 10- 11 

1.6- 10" 11 

0...3.2- 10" 3 



TABLE I 

Estimation errors averaged over all 50 reference trajectories for models A, 

B, and C. Also shown are the values of the determinant of the stochastic 

observability Gramian Q = Q T diag(W, . . . , W)~ 1 Q where Q is the 

observability matrix and W the measurement covariance matrix. 



is computed where r is the rotation matrix as in eq. (13 1. 
The estimation performance is then subsumed into the average 
over all trajectories of the maximal error of an individual 
trajectory - av^ (max^ (A,-*.)) - as well as the average over 
all trajectories of the mean error of an individual trajectory: 
avj (avfc (Ajfc)). As can be seen in the first three columns of 
table H] the errors become smaller from model A to model 
C. Going from purely relative to partially relative ("mixed") 
coordinates (A — > B) has a larger effect than going from the 
white noise jerk model for the target dynamics to the CTRA 
model in mixed coordinates (B — > C). 



C. Observability analysis 

In the introduction it was stated that the target state in 
global coordinates is unobservable if the ego vehicle has 
no absolute position measurements by e. g. GPS and that 
the target state is poorly observable if GPS -only (without 
differential corrections) measurements are available. 

On the other hand, by formulating the target dynamics in a 
relative coordinate system as in section III] the target dynamics 
might become observable if at least relative 2d position mea- 
surements are provided by the exteroceptive sensors. As can be 
only the observable quantities Vn 



seen in section 
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ego ego 
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A o 
Hence 



and ipQ 9 ° enter the vector fields in eqs. |9| [111 and 
also the induced norm of the combined input and process noise 
is bounded - another necessary condition for the convergence 
of the EKF E). 

For the observability analysis we have evaluated the deter- 
minant of the observability Gramian (see e. g. O) over all 
trajectories. The determinants for models A and B are exactly 
960400(Atfe) 12 ; this is also the expression for the simple 2d 
white noise jerk model as in [13]. The determinant for model 
C turned out to be too complex to be derived analytically and 
is given by a numerical range in table II] 

As can be seen in the last column of table [TJ models A and B 
are always observable irrespective of the state, whereas model 
C can become unobservable. This happens for example when 
speed and acceleration over ground become zero. However 
despite the intermittent ill-observability, model C performs 
best in terms of the estimation error (table [I}. 

V. CONCLUSIONS AND OUTLOOK 

In this paper a general framework for the derivation of the 
dynamical vector field of the relative target dynamics for target 
tracking based on the vector fields for the global target and ego 

s As already stated, the dependence of the process noise covariance matrix 
on Vfj ff0 disappears if the Cartesian process noise is chosen to be isotropic. 



dynamics was presented. It was also shown how non-inertial 
contributions are taken into account when relative (body-fixed) 
velocities and accelerations are part of the state vector. The 
framework was applied to three different combinations of 
target and ego dynamics and coordinate choices and their 
ability in tracking targets was assessed by a numerical study. 
Model C, the CTRA model formulated in mixed coordinates, 
i. e. with relative position and angle coordinates but global 
(over ground) angle rate, speed, and acceleration coordinates, 
turned out to be more accurate than the other two models. 

Since model C can become unobservable, its observability 
should be studied more carefully. Although a point in state 
space where the system becomes unobservable has already 
been identified, an exhaustive characterization of the unob- 
servable state and input subspace is required along with an 
analysis how those unobservable subspaces affect the overall 
tracking performance in realistic driving scenarios. 

Appendix 
A. 2D rotations and non-inertial contributions 
The 2D rotation matrix is given by 



cos(^ effo ) sm(ip e 9°) 
- sin(-0 e s°) cos(V> e9 °) 



(13) 



The transformation of a six-dimensional vector containing 
the differences between target and ego vehicle in horizontal 
position, velocity, and acceleration over ground into body fixed 
coordinates is accomplished by the matrix 



M 




(14) 



On the diagonal are the rotation matrices (13i while the off- 
diagonal terms are due to non-inertial corrections: e. g. the 
entries r and 1r give rise to centrifugal and Coriolis pseudo- 
forces, respectively, see e. g. fBl . The f-term provides a 
necessary velocity correction as can be seen in fig. [3] 
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Fig. 3. ACC target in follow mode for a circular trajectory with constant 
yaw rates and constant speeds. 

Without the correction term which can also be written in 3d 
vector notation as — ip x x* rel the relative velocity would not 
be zero. 



B. Constant turn rate and acceleration model 

A special case of a 2d curvilinear motion model Q] is 
here referred to as the constant turn rate and acceleration 
model (CTRA); its simpler version, the constant turn rate and 
velocity model (CTRV) represents the non-holonomic system 
of a vertical disk rolling on a horizontal plane lfl4"l . Both 
models are commonly used to approximate a wheeled vehicle's 
dynamics under normal driving conditions where the slip angle 
can be neglected |3|. The longitudinal acceleration is included 
in the state vector £| 9 ° = (x, y, ip, ip, v, a) T since e. g. 
ACC systems use the acceleration of the target vehicle as a 
control input. Its continuous dynamics is given by 

d ( ■ \ T 

~^g 9 ° = («cos-0, vsinip, ip, v$(t), a, i/_(*)J (15) 

The discrete-time counterpart solution 

e g 9 °{t) = F%° °(C g 9 °(to),t - to, ^ 9 °(*o)) (16) 

with fg 9 °(t) = (fj,(t), ^a(i)) T can be obtained by linear 
system techniques and is not provided here for lack of space. 
It contains Fresnel integrals which can be expanded to first 
order in v^ita) to obtain analytically tractable expressions. 
The solution also has inessential singularities at ip(to) = 
which can be removed by Taylor expansion. 
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